/*
 * ARM GDB support
 * arch-specific portion of GDB stub
 *
 * File      : arm_stub.c
 * This file is part of RT-Thread RTOS
 * COPYRIGHT (C) 2006, RT-Thread Develop Team
 *
 * The license and distribution terms for this file may be
 * found in the file LICENSE in this distribution or at
 * http://www.rt-thread.org/license/LICENSE
 *
 * Change Logs:
 * Date           Author       Notes
 * 2014-07-04     Wzyy2      first version
 */
#include <rtthread.h>
#include <rthw.h>
#include <gdb_stub.h>
#include <arch_gdb.h>

#define PS_N 0x80000000
#define PS_Z 0x40000000
#define PS_C 0x20000000
#define PS_V 0x10000000

#define IS_THUMB_ADDR(addr)     ((addr) & 1)
#define MAKE_THUMB_ADDR(addr) ((addr) | 1)
#define UNMAKE_THUMB_ADDR(addr) ((addr) & ~1)

static int compiled_break = 0;
static unsigned long step_addr = 0;
static int ins_will_execute(unsigned long ins);
static unsigned long target_ins(unsigned long* pc, unsigned long ins);


/*struct gdb_arch - Describe architecture specific values.*/
struct gdb_arch arch_gdb_ops = {
	.gdb_bpt_instr		=  {0xfe, 0xde, 0xff, 0xe7}  //Little-Endian
};

struct rt_gdb_register {
	rt_uint32_t r0;
	rt_uint32_t r1;
	rt_uint32_t r2;
	rt_uint32_t r3;
	rt_uint32_t r4;
	rt_uint32_t r5;
	rt_uint32_t r6;
	rt_uint32_t r7;
	rt_uint32_t r8;
	rt_uint32_t r9;
	rt_uint32_t r10;
	rt_uint32_t fp;
	rt_uint32_t ip;
	rt_uint32_t sp;
	rt_uint32_t lr;
	rt_uint32_t pc;
	rt_uint32_t cpsr;
	rt_uint32_t ORIG_r0;
}* regs;

/**
 * gdb_breakpoint - generate a compiled_breadk
 * It is used to sync up with a debugger and stop progarm
 */
void gdb_breakpoint()
{
	asm(".word 0xe7ffdeff");
}

void gdb_set_register(void* hw_regs)
{
	regs = (struct rt_gdb_register*)hw_regs;
}
void gdb_get_register(unsigned long* gdb_regs)
{
	int regno;

	/* Initialize all to zero. */
	for(regno = 0; regno < GDB_MAX_REGS; regno++)
		gdb_regs[regno] = 0;

	gdb_regs[GDB_R0]		= regs->r0;
	gdb_regs[GDB_R1]		= regs->r1;
	gdb_regs[GDB_R2]		= regs->r2;
	gdb_regs[GDB_R3]		= regs->r3;
	gdb_regs[GDB_R4]		= regs->r4;
	gdb_regs[GDB_R5]		= regs->r5;
	gdb_regs[GDB_R6]		= regs->r6;
	gdb_regs[GDB_R7]		= regs->r7;
	gdb_regs[GDB_R8]		= regs->r8;
	gdb_regs[GDB_R9]		= regs->r9;
	gdb_regs[GDB_R10]	    = regs->r10;
	gdb_regs[GDB_FP]		= regs->fp;
	gdb_regs[GDB_IP]		= regs->ip;
	gdb_regs[GDB_SPT]	    = regs->sp;
	gdb_regs[GDB_LR]		= regs->lr;
	gdb_regs[GDB_PC]		= regs->pc;
	gdb_regs[GDB_CPSR]  	= regs->cpsr;

};


void gdb_put_register(unsigned long* gdb_regs)
{
	regs->r0    	= gdb_regs[GDB_R0];
	regs->r1    	= gdb_regs[GDB_R1];
	regs->r2    	= gdb_regs[GDB_R2];
	regs->r3    	= gdb_regs[GDB_R3];
	regs->r4    	= gdb_regs[GDB_R4];
	regs->r5    	= gdb_regs[GDB_R5];
	regs->r6    	= gdb_regs[GDB_R6];
	regs->r7    	= gdb_regs[GDB_R7];
	regs->r8    	= gdb_regs[GDB_R8];
	regs->r9    	= gdb_regs[GDB_R9];
	regs->r10   	= gdb_regs[GDB_R10];
	regs->fp        = gdb_regs[GDB_FP];
	regs->ip    	= gdb_regs[GDB_IP];
	regs->sp    	= gdb_regs[GDB_SPT];
	regs->lr	    = gdb_regs[GDB_LR];
	regs->pc        = gdb_regs[GDB_PC];
	regs->cpsr      = gdb_regs[GDB_CPSR];
}


/* It will be called during process_packet */
int gdb_arch_handle_exception(char* remcom_in_buffer,
                              char* remcom_out_buffer)
{
	unsigned long addr, curins;
	char* ptr;

	/*clear single step*/
	if(step_addr) {
		gdb_remove_sw_break(step_addr);
		step_addr = 0;
	}

	switch(remcom_in_buffer[0]) {
		case 'D':
		case 'k':
		case 'c':
			/*
			 * If this was a compiled breakpoint, we need to move
			 * to the next instruction or we will breakpoint
			 * over and over again
			 */
			ptr = &remcom_in_buffer[1];

			if(gdb_hex2long(&ptr, &addr))
				regs->pc = addr;
			else if(compiled_break == 1)
				regs->pc += 4;

			compiled_break = 0;
			return 0;

		case 's':
			ptr = &remcom_in_buffer[1];

			if(gdb_hex2long(&ptr, &addr))
				regs->pc = addr;

			curins = *(unsigned long*)(regs->pc);

			if(ins_will_execute(curins))
				//Decode instruction to decide what the next pc will be
				step_addr = target_ins((unsigned long*)regs->pc, curins);
			else
				step_addr = regs->pc + 4;

#ifdef RT_GDB_DEBUG
			rt_kprintf("\n next will be %x \n", step_addr);
#endif
			gdb_set_sw_break(step_addr);

			if(compiled_break == 1)
				regs->pc += 4;

			compiled_break = 0;
			return 0;
	}

	return -1;

}

/* flush icache to let the sw breakpoint working */
void gdb_flush_icache_range(unsigned long start, unsigned long end)
{
#ifdef RT_GDB_ICACHE
	extern void mmu_invalidate_icache();
	mmu_invalidate_icache();  //for arm,wo can only invalidate it
#endif
}

/* register a hook in undef*/
int gdb_undef_hook(void* regs)
{
	struct rt_gdb_register* tmp_reg = (struct rt_gdb_register*)regs;
	unsigned long* tmp_pc = (unsigned long*)tmp_reg->pc;

	/* it is a compiled break */
	if(*tmp_pc == GDB_COMPILED_BREAK) {
		compiled_break = 1;
		gdb_handle_exception(SIGTRAP, regs);
		return 1;

	}
	/* it is a sw break */
	else if(*tmp_pc == GDB_BREAKINST) {
		gdb_handle_exception(SIGTRAP, regs);
		return 1;
	}

	/*or we just go */
	return 0;

}

static unsigned long gdb_arch_regs[GDB_MAX_REGS];
static int ins_will_execute(unsigned long ins)
{
	unsigned long psr = regs->cpsr;  // condition codes
	int res = 0;

	switch((ins & 0xF0000000) >> 28) {
		case 0x0: // EQ
			res = (psr & PS_Z) != 0;
			break;

		case 0x1: // NE
			res = (psr & PS_Z) == 0;
			break;

		case 0x2: // CS
			res = (psr & PS_C) != 0;
			break;

		case 0x3: // CC
			res = (psr & PS_C) == 0;
			break;

		case 0x4: // MI
			res = (psr & PS_N) != 0;
			break;

		case 0x5: // PL
			res = (psr & PS_N) == 0;
			break;

		case 0x6: // VS
			res = (psr & PS_V) != 0;
			break;

		case 0x7: // VC
			res = (psr & PS_V) == 0;
			break;

		case 0x8: // HI
			res = ((psr & PS_C) != 0) && ((psr & PS_Z) == 0);
			break;

		case 0x9: // LS
			res = ((psr & PS_C) == 0) || ((psr & PS_Z) != 0);
			break;

		case 0xA: // GE
			res = ((psr & (PS_N | PS_V)) == (PS_N | PS_V)) ||
			      ((psr & (PS_N | PS_V)) == 0);
			break;

		case 0xB: // LT
			res = ((psr & (PS_N | PS_V)) == PS_N) ||
			      ((psr & (PS_N | PS_V)) == PS_V);
			break;

		case 0xC: // GT
			res = ((psr & (PS_N | PS_V)) == (PS_N | PS_V)) ||
			      ((psr & (PS_N | PS_V)) == 0);
			res = ((psr & PS_Z) == 0) && res;
			break;

		case 0xD: // LE
			res = ((psr & (PS_N | PS_V)) == PS_N) ||
			      ((psr & (PS_N | PS_V)) == PS_V);
			res = ((psr & PS_Z) == PS_Z) || res;
			break;

		case 0xE: // AL
			res = 1;
			break;

		case 0xF: // NV
			if(((ins & 0x0E000000) >> 24) == 0xA)
				res = 1;
			else
				res = 0;

			break;
	}

	return res;
}

static unsigned long RmShifted(int shift)
{
	unsigned long Rm = gdb_arch_regs[shift & 0x00F];
	int shift_count;

	if((shift & 0x010) == 0) {
		shift_count = (shift & 0xF80) >> 7;
	} else {
		shift_count = gdb_arch_regs[(shift & 0xF00) >> 8];
	}

	switch((shift & 0x060) >> 5) {
		case 0x0: // Logical left
			Rm <<= shift_count;
			break;

		case 0x1: // Logical right
			Rm >>= shift_count;
			break;

		case 0x2: // Arithmetic right
			Rm = (unsigned long)((long)Rm >> shift_count);
			break;

		case 0x3: // Rotate right
			if(shift_count == 0) {
				// Special case, RORx
				Rm >>= 1;

				if(gdb_arch_regs[GDB_CPSR] & PS_C) Rm |= 0x80000000;
			} else {
				Rm = (Rm >> shift_count) | (Rm << (32 - shift_count));
			}

			break;
	}

	return Rm;
}

// Decide the next instruction to be executed for a given instruction
static unsigned long target_ins(unsigned long* pc, unsigned long ins)
{
	unsigned long new_pc, offset, op2;
	unsigned long Rn;
	int i, reg_count, c;

	gdb_get_register(gdb_arch_regs);

	switch((ins & 0x0C000000) >> 26) {
		case 0x0:

			// BX or BLX
			if((ins & 0x0FFFFFD0) == 0x012FFF10) {
				new_pc = (unsigned long)gdb_arch_regs[ins & 0x0000000F];
				return new_pc;
			}

			// Data processing
			new_pc = (unsigned long)(pc + 1);

			if((ins & 0x0000F000) == 0x0000F000) {
				// Destination register is PC
				if((ins & 0x0FBF0000) != 0x010F0000) {
					Rn = (unsigned long)gdb_arch_regs[(ins & 0x000F0000) >> 16];

					if((ins & 0x000F0000) == 0x000F0000) Rn += 8;   // PC prefetch!

					if((ins & 0x02000000) == 0) {
						op2 = RmShifted(ins & 0x00000FFF);
					} else {
						op2 = ins & 0x000000FF;
						i = (ins & 0x00000F00) >> 8;  // Rotate count
						op2 = (op2 >> (i * 2)) | (op2 << (32 - (i * 2)));
					}

					switch((ins & 0x01E00000) >> 21) {
						case 0x0: // AND
							new_pc = Rn & op2;
							break;

						case 0x1: // EOR
							new_pc = Rn ^ op2;
							break;

						case 0x2: // SUB
							new_pc = Rn - op2;
							break;

						case 0x3: // RSB
							new_pc = op2 - Rn;
							break;

						case 0x4: // ADD
							new_pc = Rn + op2;
							break;

						case 0x5: // ADC
							c = (gdb_arch_regs[GDB_CPSR] & PS_C) != 0;
							new_pc = Rn + op2 + c;
							break;

						case 0x6: // SBC
							c = (gdb_arch_regs[GDB_CPSR] & PS_C) != 0;
							new_pc = Rn - op2 + c - 1;
							break;

						case 0x7: // RSC
							c = (gdb_arch_regs[GDB_CPSR] & PS_C) != 0;
							new_pc = op2 - Rn + c - 1;
							break;

						case 0x8: // TST
						case 0x9: // TEQ
						case 0xA: // CMP
						case 0xB: // CMN
							break; // PC doesn't change

						case 0xC: // ORR
							new_pc = Rn | op2;
							break;

						case 0xD: // MOV
							new_pc = op2;
							break;

						case 0xE: // BIC
							new_pc = Rn & ~op2;
							break;

						case 0xF: // MVN
							new_pc = ~op2;
							break;
					}
				}
			}

			return new_pc;

		case 0x1:
			if((ins & 0x02000010) == 0x02000010) {
				// Undefined!
				return (unsigned long)(pc + 1);
			} else {
				if((ins & 0x00100000) == 0) {
					// STR
					return (unsigned long)(pc + 1);
				} else {
					// LDR
					if((ins & 0x0000F000) != 0x0000F000) {
						// Rd not PC
						return (unsigned long)(pc + 1);
					} else {
						Rn = (unsigned long)gdb_arch_regs[(ins & 0x000F0000) >> 16];

						if((ins & 0x000F0000) == 0x000F0000) Rn += 8;   // PC prefetch!

						if(ins & 0x01000000) {
							// Add/subtract offset before
							if((ins & 0x02000000) == 0) {
								// Immediate offset
								if(ins & 0x00800000) {
									// Add offset
									Rn += (ins & 0x00000FFF);
								} else {
									// Subtract offset
									Rn -= (ins & 0x00000FFF);
								}
							} else {
								// Offset is in a register
								if(ins & 0x00800000) {
									// Add offset
									Rn += RmShifted(ins & 0x00000FFF);
								} else {
									// Subtract offset
									Rn -= RmShifted(ins & 0x00000FFF);
								}
							}
						}

						return *(unsigned long*)Rn;
					}
				}
			}

			return (unsigned long)(pc + 1);

		case 0x2:  // Branch, LDM/STM
			if((ins & 0x02000000) == 0) {
				// LDM/STM
				if((ins & 0x00100000) == 0) {
					// STM
					return (unsigned long)(pc + 1);
				} else {
					// LDM
					if((ins & 0x00008000) == 0) {
						// PC not in list
						return (unsigned long)(pc + 1);
					} else {
						Rn = (unsigned long)gdb_arch_regs[(ins & 0x000F0000) >> 16];

						if((ins & 0x000F0000) == 0x000F0000) Rn += 8;   // PC prefetch!

						offset = ins & 0x0000FFFF;
						reg_count = 0;

						for(i = 0;  i < 15;  i++) {
							if(offset & (1 << i)) reg_count++;
						}

						if(ins & 0x00800000) {
							// Add offset
							Rn += reg_count * 4;
						} else {
							// Subtract offset
							Rn -= 4;
						}

						return *(unsigned long*)Rn;
					}
				}
			} else {
				// Branch
				if(ins_will_execute(ins)) {
					offset = (ins & 0x00FFFFFF) << 2;

					if(ins & 0x00800000)
						offset |= 0xFC000000;  // sign extend

					new_pc = (unsigned long)(pc + 2) + offset;

					// If its BLX, make new_pc a thumb address.
					if((ins & 0xFE000000) == 0xFA000000) {
						if((ins & 0x01000000) == 0x01000000)
							new_pc |= 2;

						new_pc = MAKE_THUMB_ADDR(new_pc);
					}

					return new_pc;
				} else {
					// Falls through
					return (unsigned long)(pc + 1);
				}
			}

		case 0x3:  // Coprocessor & SWI
			if(((ins & 0x03000000) == 0x03000000) && ins_will_execute(ins)) {
				// SWI
				// TODO(wzyy2) some problems.
				extern unsigned long vector_swi;
				return vector_swi;
			} else {
				return (unsigned long)(pc + 1);
			}

		default:
			// Never reached - but fixes compiler warning.
			return 0;
	}
}
